﻿using System;
using System.Collections;
using System.Collections.Generic;

namespace Hont.AStar
{
    public class HontAStar
    {
        IGrid mIGrid;
        Grid mGrid;
        Node mBeginNode;
        Node mTargetNode;
        List<Node> mOpenList;
        BitArray mOpenContainDetecteList;
        /// <summary>
        /// Walked road and closed road.
        /// </summary>
        BitArray mClosedList;
        /// <summary>
        /// 'H' Cost estimate interface.
        /// </summary>
        IAStarHCost mHCost;
        /// <summary>
        /// The straight cost.
        /// </summary>
        int mGStraightCost;
        /// <summary>
        /// The diagonal cost.
        /// </summary>
        int mGDiagonalCost;
        IAStarStepDebuger mDebuger;

        public Grid Grid { get { return mGrid; } }
        public IAStarStepDebuger Debuger { get { return mDebuger; } set { mDebuger = value; } }


        public HontAStar() : this(new EasyAStarHCost())
        {
        }

        public HontAStar(IAStarHCost aStarCost) : this(aStarCost, 14, 10)
        {
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="hCost">'H' Cost estimate interface.</param>
        /// <param name="straightCost">The straight road cost.</param>
        /// <param name="diagonalCost">The diagonal road cost.</param>
        public HontAStar(IAStarHCost hCost, int straightCost, int diagonalCost)
        {
            this.mHCost = hCost;
            this.mGStraightCost = straightCost;
            this.mGDiagonalCost = diagonalCost;

            mDebuger = new EmptyAStarDebuger();
        }

        public void Init(Grid grid, int buildPathCapacity = 10)
        {
            this.mGrid = grid;
            this.mIGrid = mGrid;

            mOpenList = new List<Node>(mGrid.Lenght * mGrid.Width * mGrid.Height);
            mOpenContainDetecteList = new BitArray(mGrid.Lenght * mGrid.Width * mGrid.Height);
            mClosedList = new BitArray(mGrid.Lenght * mGrid.Width * mGrid.Height);
        }

        /// <summary>
        /// Start pathfinding, if not walkable path will return null.
        /// </summary>
        public Position[] Start(Position startPos, Position endPos, int mask = -1, bool combinePath = true)
        {
            mDebuger.OnPathfindingBegin(startPos, endPos);

            mOpenList.Clear();
            mOpenContainDetecteList.SetAll(false);
            mClosedList.SetAll(false);
            mBeginNode = mIGrid[startPos.X, startPos.Y, startPos.Z];
            mTargetNode = mIGrid[endPos.X, endPos.Y, endPos.Z];

            //Init first node.
            mBeginNode.Init(0, mHCost.GetCost(mBeginNode.Position, mTargetNode.Position), mBeginNode.Walkable, 1);

            var searchResult = Search(mask);
            if (searchResult)
            {
                var positions = Node2PositionArray(mTargetNode);

                if (combinePath)
                    return HontAStarHelper.CombinePath(positions);
                else
                    return positions;
            }

            return null;
        }

        /// <summary>
        /// Start pathfinding, if not walkable path will return null.
        /// </summary>
        public int Start_NonAlloc(Position startPos, Position endPos, ref Position[] cacheResultArray, int mask = -1, bool combinePath = true)
        {
            var result = -1;

            mDebuger.OnPathfindingBegin(startPos, endPos);

            mOpenList.Clear();
            mOpenContainDetecteList.SetAll(false);
            mClosedList.SetAll(false);
            mBeginNode = mIGrid[startPos.X, startPos.Y, startPos.Z];
            mTargetNode = mIGrid[endPos.X, endPos.Y, endPos.Z];

            //Init first node.
            mBeginNode.Init(0, mHCost.GetCost(mBeginNode.Position, mTargetNode.Position), mBeginNode.Walkable, 1);

            var searchResult = Search(mask);
            if (searchResult)
            {
                result = Node2PositionArray_NonAlloc(mTargetNode, ref cacheResultArray);

                if (combinePath)
                {
                    result = HontAStarHelper.CombinePath_NonAlloc(cacheResultArray, result, ref cacheResultArray);
                }
            }

            return result;
        }

        /// <summary>
        /// Check node is walkable.
        /// </summary>
        bool NodeIsWalkable(Node currentNode, Node compareNode, int mask)
        {
            bool result = false;

            if (compareNode.Walkable
                && compareNode.CheckMask(mask)
                && !mClosedList[compareNode.ID])
            {
                var adjacentPoint1 = mIGrid[currentNode.X, compareNode.Y, compareNode.Z];
                var adjacentPoint2 = mIGrid[compareNode.X, currentNode.Y, compareNode.Z];
                var adjacentPoint3 = mIGrid[compareNode.X, compareNode.Y, currentNode.Z];

                if (adjacentPoint1.Walkable && adjacentPoint1.CheckMask(mask)
                    && adjacentPoint2.Walkable && adjacentPoint2.CheckMask(mask)
                    && adjacentPoint3.Walkable && adjacentPoint3.CheckMask(mask))
                {
                    if (!mOpenContainDetecteList[compareNode.ID])
                        result = true;
                }
            }

            return result;
        }

        int GetGValue(Node currentNode, Node compareNode)
        {
            var result = mGDiagonalCost;

            var dirX = compareNode.Position.X - currentNode.Position.X;
            var dirY = compareNode.Position.Y - currentNode.Position.Y;
            var dirZ = compareNode.Position.Z - currentNode.Position.Z;

            if (dirX * dirX + dirY * dirY + dirZ * dirZ == 1)
            {
                result = mGStraightCost;
            }

            result += currentNode.G + compareNode.Cost;

            return result;
        }

        bool Search(int mask)
        {
            var result = true;
            var currentNode = mBeginNode;
            mClosedList[mBeginNode.ID] = true;
            mDebuger.OnAddToClosedList(mBeginNode.Position);

            while (currentNode != mTargetNode && result)
            {
                var startX = Math.Max(0, currentNode.X - 1);
                var endX = Math.Min(mGrid.Lenght - 1, currentNode.X + 1);
                var startY = Math.Max(0, currentNode.Y - 1);
                var endY = Math.Min(mGrid.Height - 1, currentNode.Y + 1);
                var startZ = Math.Max(0, currentNode.Z - 1);
                var endZ = Math.Min(mGrid.Width - 1, currentNode.Z + 1);

                //Compare around 27 direction.
                for (int i = startX; i <= endX; i++)
                {
                    for (int j = startY; j <= endY; j++)
                    {
                        for (int k = startZ; k <= endZ; k++)
                        {
                            var compareNode = mIGrid[i, j, k];
                            mDebuger.OnToNextPoint(compareNode.Position, DebugerNodeTypeEnum.CompareNode);
                            if (!NodeIsWalkable(currentNode, compareNode, mask)) continue;

                            compareNode.G = GetGValue(currentNode, compareNode);
                            compareNode.H = mHCost.GetCost(compareNode.Position, mTargetNode.Position);
                            compareNode.Parent = currentNode;
                            mOpenList.Add(compareNode);
                            mOpenContainDetecteList[compareNode.ID] = true;
                        }
                    }
                }

                result = mOpenList.Count > 0;//Has remain road.

                if (result)
                {
                    /// Sort OpenList and through 'f' value. return first node.
                    mOpenList.Sort((x, y) => x.F.CompareTo(y.F));
                    currentNode = mOpenList[0];
                    mOpenList.Remove(currentNode);
                    mOpenContainDetecteList[currentNode.ID] = false;
                    mClosedList[currentNode.ID] = true;
                    mDebuger.OnAddToClosedList(currentNode.Position);

                    mDebuger.OnToNextPoint(currentNode.Position, DebugerNodeTypeEnum.NextNode);
                }
            }

            return result;
        }

        int Node2PositionArray_NonAlloc(Node node, ref Position[] cacheResultArray)
        {
            var tNode = node;
            var elementCount = 0;

            while (tNode != mBeginNode)
            {
                tNode = tNode.Parent;

                cacheResultArray[elementCount] = tNode.Position;
                elementCount++;
            }

            var a = cacheResultArray;
            var length = elementCount;
            for (var i = 0; i < length / 2; i++)
            {
                var temp = a[i];
                a[i] = a[length - i - 1];
                a[length - i - 1] = temp;
            }

            return elementCount;
        }

        Position[] Node2PositionArray(Node node)
        {
            var resultList = new List<Position>();
            var tNode = node;

            while (tNode != mBeginNode)
            {
                tNode = tNode.Parent;

                resultList.Add(tNode.Position);
            }

            resultList.Reverse();

            return resultList.ToArray();
        }
    }
}
